Lab 7: Kalman Filter

1. Estimate Drag and Momentum

To build the state-space model for the Kalman filter, I first needed to estimate the effective drag term d and momentum term m of the robot. Following the lab procedure, I did this using a step response. In this test, I drove the robot toward a wall with a 60% constant motor input and logged the ToF sensor output over time. The goal was to find the robot’s steady-state speed and its 90% rise time, since those values can be used to estimate d and m.

The simplified 1D model used in lecture is

\[ m\ddot{x} = u - d\dot{x} \]

or equivalently,

\[ m\ddot{x} + d\dot{x} = u \]

At steady state, acceleration is zero, so:

\[ d = \frac{u}{\dot{x}_{ss}} \]

where \dot{x}_{ss} is the steady-state speed. The momentum term was estimated from the 90% rise time using

\[ m = -\frac{d\,t_{0.9}}{\ln(1-0.9)} \]

The raw ToF data is discrete and stair stepped, so directly differentiating it produced noisy speed spikes. To get a more reliable speed estimate, I used only the fresh ToF samples, converted time to seconds, lightly smoothed the distance data, and then computed speed from the slope of the smoothed position signal.

The figure shows both the ToF distance and computed speed over time. The distance curve starts slightly curved and becomes more linear, showing the robot speeds up and then moves at a near-constant velocity. The speed plot rises from near zero and levels off, which indicates a steady-state region used to estimate the final speed and rise time.

the figure above shows the speed plot again with the steady-state speed, 90% of steady-state, and the selected 90% rise-time point marked.

From the processed data, I first computed the robot speed by taking the derivative of the smoothed ToF distance. I then selected a steady-state region near the end of the run where the speed was approximately constant, and averaged it to get:

\[ \dot{x}_{ss} = 2039.370 \text{ mm/s} \]

Next, I computed 90% of the steady-state speed:

\[ 0.9\dot{x}_{ss} = 1835.433 \text{ mm/s} \]

I found the first time the speed reached this value, which gave:

\[ \dot{x}(t_{0.9}) = 1871.015 \text{ mm/s}, \quad t_{0.9} = 1.044 \text{ s} \]

Using the model \(m\ddot{x} + d\dot{x} = u\), I computed:

\[ d = \frac{u}{\dot{x}_{ss}} = \frac{0.6}{2039.370} = 0.000294 \]

\[ m = -\frac{d\,t_{0.9}}{\ln(0.1)} = 0.000133 \]

These values are for building the state-space model for the Kalman filter later.

2. Initialize KF (Python)

After estimating the drag \(d\) and momentum \(m\) from Task 1, I used them to build a state-space model for the robot. The state is defined as position and velocity:

\[ x = \begin{bmatrix} x \\ \dot{x} \end{bmatrix} \]

The system matrix is:

\[ A = \begin{bmatrix} 0 & 1 \\ 0 & -d/m \end{bmatrix} \]

and the input matrix is:

\[ B = \begin{bmatrix} 0 \\ 1/m \end{bmatrix} \]

Since the ToF sensor measures distance to the wall, the measurement matrix is:

\[ C = [-1 \quad 0] \]

Using the values from Task 1, I get:

\[ d = 0.000294, \quad m = 0.000133 \]

\[ A = \begin{bmatrix} 0 & 1 \\ 0 & -2.2105 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 7518.7970 \end{bmatrix} \]

The sampling time was computed from the ToF data:

\[ \Delta t = 0.099895 \text{ s} \]

The discrete system is:

\[ A_d = I + \Delta t A, \quad B_d = \Delta t B \]

\[ A_d = \begin{bmatrix} 1 & 0.0999 \\ 0 & 0.7792 \end{bmatrix}, \quad B_d = \begin{bmatrix} 0 \\ 751.0882 \end{bmatrix} \]

The initial state uses the first ToF measurement and zero velocity:

\[ x_0 = \begin{bmatrix} -2885 \\ 0 \end{bmatrix} \]

The initial covariance is:

\[ P_0 = \begin{bmatrix} 10000 & 0 \\ 0 & 90000 \end{bmatrix} \]

The measurement noise was set based on ToF uncertainty:

\[ \sigma_3 = 20 \text{ mm} \]

The process noise was chosen based on the lecture scale:

\[ \sigma_1 = \sigma_2 = 31.639 \]

\[ \Sigma_u = \begin{bmatrix} 1001.05 & 0 \\ 0 & 1001.05 \end{bmatrix}, \quad \Sigma_z = \begin{bmatrix} 400 \end{bmatrix} \]

Python code implementation:

A = np.array([
    [0.0, 1.0],
    [0.0, -d_est / m_est]
])
B = np.array([
    [0.0],
    [1.0 / m_est]
])
C = np.array([[-1.0, 0.0]])

Ad = np.eye(2) + dt * A
Bd = dt * B

x0 = np.array([
    [-tof[0]],
    [0.0]
])
P0 = np.array([
    [100.0**2, 0.0],
    [0.0, 300.0**2]
])

sig_u = np.array([
    [sigma_1**2, 0.0],
    [0.0, sigma_2**2]
])
sig_z = np.array([[sigma_3**2]])

3. Implement and test Kalman Filter in Jupyter

To verify that the Kalman Filter works correctly, I implemented it in Python using the matrices from Task 2 and applied it to the same straight-run dataset collected in Task 1. The dataset includes time, ToF distance measurements, and the motor input.

Before running the filter, I formatted the data so that all input arrays had the same length and were aligned in time. The motor input was also scaled to match the step size used during the experiment. I then looped through the dataset and applied the Kalman Filter at each time step.

A simplified version of the implementation is shown below:

def kf(mu, sigma, u, y, Ad, Bd, C, Sigma_u, Sigma_z):
    mu_p = Ad @ mu + Bd * u
    sigma_p = Ad @ sigma @ Ad.T + Sigma_u

    sigma_m = C @ sigma_p @ C.T + Sigma_z
    kkf_gain = sigma_p @ C.T @ np.linalg.inv(sigma_m)

    y_m = y - C @ mu_p
    mu = mu_p + kkf_gain @ y_m
    sigma = (np.eye(2) - kkf_gain @ C) @ sigma_p

    return mu, sigma

The filter was applied across the dataset as follows:

mu = x0.copy()
sigma = P0.copy()

mu_hist = []

for k in range(len(t)):
    u_k = float(u_kf[k])
    y_k = np.array([[float(-tof[k])]])

    mu, sigma = kf(mu, sigma, u_k, y_k, Ad, Bd, C, sig_u, sig_z)
    mu_hist.append(mu.flatten())

The Kalman Filter combines the system model with noisy sensor measurements to estimate both distance and velocity. To improve performance, I tuned the measurement noise parameter.

\[ \sigma_3 = 3 \text{ mm} \]

\[ \Sigma_z = 9 \]

With this tuning, the filter achieved a mean distance error of:

\[ \text{Mean error} = 3.31 \text{ mm} \]

Because the estimated velocity is not directly measured, the figure shows an initial transient where the value starts higher than expected and then decreases before stabilizing. This is due to the initial state and small inconsistencies in the first few ToF measurements. After approximately 0.3–0.5 seconds, the estimate converges and remains stable.

4. Implement the Kalman Filter on the Robot

In this task, I created a KF.hpp file and integrated the Kalman Filter into my existing Lab 5 control system on the Artemis. The purpose of this file was to move the Kalman Filter from Python simulation to the real robot, so the robot could estimate its distance to the wall and its velocity in real time while driving.

The Kalman Filter in KF.hpp stores the state estimate and covariance, create the prediction step using the previous motor command, and then updates the estimate using the newest ToF measurement. This allows the robot to estimate its motion continuously instead of relying only on slow and noisy ToF sensor updates. The filter is called in control_update(), where the previous control input and current ToF reading are passed into the Kalman Filter before computing the PID error:

// control.hpp
const float raw_dist_mm = control_get_distance_mm();

kf_step(g_pid.last_u, raw_dist_mm);

const float est_dist_mm = kf_get_estimated_distance_mm();
const float est_vel_mm_s = kf_get_estimated_velocity_mm_s();

const float control_dist_mm = (est_dist_mm > 0.0f)
    ? ((est_dist_mm < raw_dist_mm) ? est_dist_mm : raw_dist_mm)
    : raw_dist_mm;

const float error = g_pid.setpoint_mm - control_dist_mm;

Inside KF.hpp, the Kalman Filter follows the standard prediction and update structure. The previous input is used to predict the next state, and the newest ToF measurement is used to correct the prediction.

To test the onboard Kalman Filter, I ran the robot toward a wall as fast as possible using a simple P controller. The goal of this test was not to fully tune the controller, but to verify that the Kalman Filter could still provide a stable and accurate estimate during fast motion. During each run, I logged the raw ToF distance, the Kalman Filter estimated distance, the estimated velocity, and the control input over BLE.

From the results, the Kalman Filter estimated distance follows the raw ToF signal closely while producing a smoother estimate. This can be seen in the plot, where the red KF distance curve tracks the blue raw ToF measurements very closely throughout the run. The estimated velocity is also physically reasonable. It is large at the beginning when the robot accelerates toward the wall, then decreases as the robot slows down, and changes sign when the robot overshoots and begins correcting its position.

The logged error values also show that the filter performs well. The mean error was approximately -22.98 mm, the mean absolute error was 37.66 mm, and the maximum absolute error was 94.05 mm. These values showing that the onboard Kalman Filter remained close to the measured wall distance even during fast motion.

Conclusion

The Kalman Filter improved the robot’s estimation compared to the linear extrapolation in Lab 5 by combining a model with noisy sensor data to produce smoother and more reliable distance and velocity estimates. It was really cool to implement this on a real robot and see it work in real time, especially when running the robot at higher speed. Even though there is still some small error, the overall system feels more stable and responsive than before.